#include "pinctrl.h"
#include "soc_osal.h"
#include "spi.h"
#include "gpio.h"
#include "osal_debug.h"
#include "cmsis_os2.h"
#include "app_init.h"
#include "pwm.h"

#define SPI_SLAVE_NUM                   1
#define SPI_FREQUENCY                   2
#define SPI_CLK_POLARITY                1
#define SPI_CLK_PHASE                   1
#define SPI_FRAME_FORMAT                0
#define SPI_FRAME_FORMAT_STANDARD       0
#define SPI_FRAME_SIZE_8                0x1f
#define SPI_TMOD                        0
#define SPI_WAIT_CYCLES                 0x10

#define SPI_TASK_STACK_SIZE             0x1000
#define SPI_READ_DELAY_MS               20      // 20ms采样率
#define SPI_TASK_PRIO                   (osPriority_t)(17)
#define SPI_RX_BUFFER_SIZE              14      // 接收14字节数据

/* PWM配置 */
#define PWM_LEFT_MOTOR_CHANNEL          0       // 左电机PWM通道
#define PWM_RIGHT_MOTOR_CHANNEL         1       // 右电机PWM通道
#define PWM_VERTICAL_SERVO_CHANNEL      2       // 垂直舵机PWM通道
#define PWM_HORIZONTAL_SERVO_CHANNEL    3       // 水平舵机PWM通道
#define PWM_FREQ                        50      // PWM频率(Hz)
#define MOTOR_MAX_DUTY                  90      // 电机最大占空比(%)
#define SERVO_MIN_PULSE_WIDTH           500     // 舵机最小脉冲宽度(us)
#define SERVO_MAX_PULSE_WIDTH           2500    // 舵机最大脉冲宽度(us)

/* 摇杆死区设置 */
#define JOYSTICK_CENTER                 127     // 摇杆中心值
#define JOYSTICK_DEADZONE               15      // 死区范围

/* 控制模式 */
#define MODE_MOTOR_CONTROL              0       // 电机控制模式
#define MODE_WATER_SAMPLING             1       // 水样采集模式

/* 电机方向 */
#define MOTOR_FORWARD                   0
#define MOTOR_REVERSE                   1

/* 全局变量 */
static uint8_t g_spi_rx_buffer[SPI_RX_BUFFER_SIZE];
static uint8_t g_control_mode = MODE_MOTOR_CONTROL;
static uint8_t g_motor_start_flag = 0;
static uint8_t g_water_measure_flag = 0;
static double g_water_depth_val = 0;
static double g_water_sample_val = 0;

/* SPI相关函数 */
static void app_spi_init_pin(void)
{
	if (CONFIG_SPI_MASTER_BUS_ID == 0) {
		uapi_pin_set_mode(CONFIG_SPI_DI_MASTER_PIN, 3);
		uapi_pin_set_mode(CONFIG_SPI_DO_MASTER_PIN, 3);
		uapi_pin_set_mode(CONFIG_SPI_CLK_MASTER_PIN, 3);
		uapi_pin_set_mode(CONFIG_SPI_CS_MASTER_PIN, 0);
		uapi_gpio_set_dir(CONFIG_SPI_CS_MASTER_PIN, GPIO_DIRECTION_OUTPUT);
		uapi_gpio_set_val(CONFIG_SPI_CS_MASTER_PIN, GPIO_LEVEL_HIGH);
	}
}

static void app_spi_master_init_config(void)
{
	spi_attr_t config = { 0 };
	spi_extra_attr_t ext_config = { 0 };
	
	config.is_slave = false;
	config.slave_num = SPI_SLAVE_NUM;
	config.bus_clk = 32000000;
	config.freq_mhz = SPI_FREQUENCY;
	config.clk_polarity = SPI_CLK_POLARITY;
	config.clk_phase = SPI_CLK_PHASE;
	config.frame_format = SPI_FRAME_FORMAT;
	config.spi_frame_format = HAL_SPI_FRAME_FORMAT_STANDARD;
	config.frame_size = HAL_SPI_FRAME_SIZE_8;
	config.tmod = SPI_TMOD;
	config.sste = 1;
	
	ext_config.qspi_param.wait_cycles = SPI_WAIT_CYCLES;
	uapi_spi_init(CONFIG_SPI_MASTER_BUS_ID, &config, &ext_config);
}

static int32_t app_spi_read_data(uint8_t *rx_buffer, uint32_t len)
{
	int32_t ret;
	uint8_t tx_buffer[2] = {0x01, 0x10}; // 假设发送固定命令获取数据
	
	/* 片选使能 */
	uapi_gpio_set_val(CONFIG_SPI_CS_MASTER_PIN, GPIO_LEVEL_LOW);
	
	/* 发送命令 */
	ret = uapi_spi_send(CONFIG_SPI_MASTER_BUS_ID, tx_buffer, 2);
	if (ret != 0) {
		osal_printk("SPI send failed: %d\n", ret);
		goto exit;
	}
	
	/* 接收数据 */
	ret = uapi_spi_recv(CONFIG_SPI_MASTER_BUS_ID, rx_buffer, len);
	if (ret != 0) {
		osal_printk("SPI receive failed: %d\n", ret);
		goto exit;
	}
	
	exit:
	/* 片选禁用 */
	uapi_gpio_set_val(CONFIG_SPI_CS_MASTER_PIN, GPIO_LEVEL_HIGH);
	return ret;
}

/* PWM控制函数 */
static void pwm_init(void)
{
	uapi_pwm_deinit();
	uapi_pwm_init();
	
	// 配置PWM频率
	pwm_config_t cfg = {
		.low_duration = 0,
		.high_duration = 50, // 初始占空比50%
		.period = 1000000 / PWM_FREQ, // 周期 = 1/freq (us)
		.polarity = 0,
		.enable = true
	};
	
	// 初始化各个PWM通道
	uapi_pwm_open(PWM_LEFT_MOTOR_CHANNEL, &cfg);
	uapi_pwm_open(PWM_RIGHT_MOTOR_CHANNEL, &cfg);
	uapi_pwm_open(PWM_VERTICAL_SERVO_CHANNEL, &cfg);
	uapi_pwm_open(PWM_HORIZONTAL_SERVO_CHANNEL, &cfg);
}

/* 电机控制函数 */
static void motor_control(uint8_t channel, uint8_t speed, uint8_t direction)
{
	// 限制速度在有效范围内
	if (speed > 100) speed = 100;
	
	// 将百分比转换为占空比值
	uint32_t duty = (uint32_t)(speed * MOTOR_MAX_DUTY / 100);
	
	// 根据方向调整PWM
	if (direction == MOTOR_FORWARD) {
		// 正向
		uapi_pwm_set_duty(channel, duty);
	} else {
		// 反向 (假设通过不同通道或极性控制)
		uapi_pwm_set_duty(channel, 100 - duty);
	}
}

/* 舵机控制函数 */
static void servo_control(uint8_t channel, uint8_t position)
{
	// 将0-255的值映射到舵机脉冲宽度(500-2500us)
	uint32_t pulse_width = SERVO_MIN_PULSE_WIDTH + 
	(uint32_t)((position * (SERVO_MAX_PULSE_WIDTH - SERVO_MIN_PULSE_WIDTH)) / 255);
	
	// 计算占空比 (以周期的百分比表示)
	uint32_t period = 1000000 / PWM_FREQ; // 周期(us)
	uint32_t duty = (pulse_width * 100) / period;
	
	// 设置PWM占空比
	uapi_pwm_set_duty(channel, duty);
}

/* 处理SPI接收到的控制数据 */
static void process_control_data(void)
{
	// 更新控制标志
	g_motor_start_flag = g_spi_rx_buffer[6];   // 左自锁按钮：启动电机
	g_water_measure_flag = g_spi_rx_buffer[7]; // 右自锁按钮：取水样模式
	
	// 更新控制模式
	if (g_motor_start_flag && !g_water_measure_flag) {
		g_control_mode = MODE_MOTOR_CONTROL;
	} else if (!g_motor_start_flag && g_water_measure_flag) {
		g_control_mode = MODE_WATER_SAMPLING;
	}
	
	// 获取摇杆值
	int8_t left_y  = (int8_t)g_spi_rx_buffer[10] - JOYSTICK_CENTER; // 左摇杆上下
	int8_t left_x  = (int8_t)g_spi_rx_buffer[11] - JOYSTICK_CENTER; // 左摇杆左右
	int8_t right_y = (int8_t)g_spi_rx_buffer[12] - JOYSTICK_CENTER; // 右摇杆上下
	int8_t right_x = (int8_t)g_spi_rx_buffer[13] - JOYSTICK_CENTER; // 右摇杆左右
	
	// 应用死区
	if (abs(left_y) < JOYSTICK_DEADZONE) left_y = 0;
	if (abs(left_x) < JOYSTICK_DEADZONE) left_x = 0;
	if (abs(right_y) < JOYSTICK_DEADZONE) right_y = 0;
	if (abs(right_x) < JOYSTICK_DEADZONE) right_x = 0;
	
	// 根据当前模式执行相应操作
	switch (g_control_mode) {
	case MODE_MOTOR_CONTROL:
		// 电机控制模式
		if (g_motor_start_flag) {
			// 左摇杆上下控制左电机
			if (left_y != 0) {
				uint8_t speed = (uint8_t)abs(left_y * 100 / JOYSTICK_CENTER);
				uint8_t direction = (left_y > 0) ? MOTOR_FORWARD : MOTOR_REVERSE;
				motor_control(PWM_LEFT_MOTOR_CHANNEL, speed, direction);
			} else {
				// 停止左电机
				motor_control(PWM_LEFT_MOTOR_CHANNEL, 0, MOTOR_FORWARD);
			}
			
			// 右摇杆上下控制右电机
			if (right_y != 0) {
				uint8_t speed = (uint8_t)abs(right_y * 100 / JOYSTICK_CENTER);
				uint8_t direction = (right_y > 0) ? MOTOR_FORWARD : MOTOR_REVERSE;
				motor_control(PWM_RIGHT_MOTOR_CHANNEL, speed, direction);
			} else {
				// 停止右电机
				motor_control(PWM_RIGHT_MOTOR_CHANNEL, 0, MOTOR_FORWARD);
			}
		} else {
			// 停止所有电机
			motor_control(PWM_LEFT_MOTOR_CHANNEL, 0, MOTOR_FORWARD);
			motor_control(PWM_RIGHT_MOTOR_CHANNEL, 0, MOTOR_FORWARD);
		}
		break;
		
	case MODE_WATER_SAMPLING:
		// 水样采集模式
		if (g_water_measure_flag) {
			// 左摇杆上下控制垂直舵机
			if (left_y != 0) {
				// 将-127~127的值映射到0~255
				uint8_t position = (uint8_t)((left_y + JOYSTICK_CENTER) * 255 / (JOYSTICK_CENTER * 2));
				servo_control(PWM_VERTICAL_SERVO_CHANNEL, position);
				
				// 更新水深值
				if (left_y > 0) {
					if (g_water_depth_val < 100) g_water_depth_val += 0.2;
				} else {
					if (g_water_depth_val > 0) g_water_depth_val -= 0.2;
				}
			}
			
			// 左摇杆左右控制水平舵机
			if (left_x != 0) {
				// 将-127~127的值映射到0~255
				uint8_t position = (uint8_t)((left_x + JOYSTICK_CENTER) * 255 / (JOYSTICK_CENTER * 2));
				servo_control(PWM_HORIZONTAL_SERVO_CHANNEL, position);
			}
			
			// 右摇杆上下控制水样采集
			if (right_y != 0) {
				if (right_y > 0) {
					if (g_water_sample_val < 100) g_water_sample_val += 2;
				} else {
					if (g_water_sample_val > 0) g_water_sample_val -= 2;
				}
			}
		}
		break;
		
	default:
		// 停止所有输出
		motor_control(PWM_LEFT_MOTOR_CHANNEL, 0, MOTOR_FORWARD);
		motor_control(PWM_RIGHT_MOTOR_CHANNEL, 0, MOTOR_FORWARD);
		servo_control(PWM_VERTICAL_SERVO_CHANNEL, JOYSTICK_CENTER);
		servo_control(PWM_HORIZONTAL_SERVO_CHANNEL, JOYSTICK_CENTER);
		break;
	}
	
	// 打印当前状态(调试用)
	osal_printk("Mode: %s, L_Motor: %d%%, R_Motor: %d%%, Depth: %.1f%%, Sample: %.1f%%\n",
		(g_control_mode == MODE_MOTOR_CONTROL) ? "Motor" : "Sampling",
		left_y, right_y, g_water_depth_val, g_water_sample_val);
}

static void *spi_pwm_task(const char *arg)
{
	unused(arg);
	
	/* 初始化SPI和PWM */
	app_spi_init_pin();
	app_spi_master_init_config();
	pwm_init();
	
	while (1) {
		/* 从SPI读取控制数据 */
		if (app_spi_read_data(g_spi_rx_buffer, SPI_RX_BUFFER_SIZE) == 0) {
			/* 处理控制数据并更新PWM输出 */
			process_control_data();
		}
		
		/* 延时 */
		osal_msleep(SPI_READ_DELAY_MS);
	}
	
	return NULL;
}

static void spi_pwm_entry(void)
{
	osThreadAttr_t attr;
	
	attr.name = "SpiPwmTask";
	attr.attr_bits = 0U;
	attr.cb_mem = NULL;
	attr.cb_size = 0U;
	attr.stack_mem = NULL;
	attr.stack_size = SPI_TASK_STACK_SIZE;
	attr.priority = SPI_TASK_PRIO;
	
	if (osThreadNew((osThreadFunc_t)spi_pwm_task, NULL, &attr) == NULL) {
		osal_printk("Create SPI-PWM task failed!\n");
	}
}

/* Run the spi_pwm_entry. */
app_run(spi_pwm_entry);

